/**
 @file controladoraPololu6.c
 @brief Decsarrollo de las las funciones para el manejo de la placa controladora POLOLU6.
 @date Septiembre, 2011.
 @author José Delgado Pérez josedelgado@ula.ve josed43@gmail.com.

 Algunos datos: La controladora POLOLU6 controla 6 servomotores digitales y trabaja vajo los protocolos USB, I2C, SERIAL.
		Las siguientes funciones permiten la manipulacion de controladora por el USB en lenguaje C usando la libreria libusb.h.
		Los drivers de reconocimientos de esta controladora estan incluidos en el Kernel linux apartir de la version 2.6 
		los drivers pueden ser descargados desde la pagina del fabricante. http://www.pololu.com/.
*/


#include "controladoraPololu6.h"


/******Funiones de manipulacion del controlador POLOLU6******/

int comprobacionDeDispositivo(libusb_device *dispositivo, unsigned short idProvedor, unsigned short idProducto){
	struct libusb_device_descriptor descripcion;
	libusb_get_device_descriptor(dispositivo, &descripcion);
	if(idProvedor == descripcion.idVendor && idProducto == descripcion.idProduct){
		return 0;
	}else{
		return -1;
	} 
}

/************************************************************/

int asignarPosicion(float position, int servo){
	const unsigned short idProvedor = 0x1ffb;
	unsigned short arregloIdProductos[]={0x0089, 0x008a, 0x008b, 0x008c};	
	libusb_context *ctx=0;
	libusb_device **listaDispositivos=0;
	libusb_init(&ctx);
	int contador = libusb_get_device_list(ctx, &listaDispositivos);
	int i,id;
	for(i=0; i<contador ; i++){
		libusb_device *dispositivo = listaDispositivos[i];
		for(id=0 ; id<4 ; id++){
			if(comprobacionDeDispositivo(dispositivo, idProvedor, arregloIdProductos[id]) == 0){
				libusb_device_handle *dispositivoManejable;
				libusb_open(dispositivo, &dispositivoManejable);
				libusb_control_transfer(dispositivoManejable,0x40,REQUEST_SET_TARGET,position*4,servo,0,0,(ushort)5000);
				libusb_close(dispositivoManejable);
				break;
			}
		}
	}
	libusb_free_device_list(listaDispositivos, 0);
	libusb_exit(ctx);
	return 0;
}

/************************************************************/

int asignarRangoMin(float rangoMin, int servo){
	const unsigned short idProvedor = 0x1ffb;
	unsigned short arregloIdProductos[]={0x0089, 0x008a, 0x008b, 0x008c};	
	libusb_context *ctx=0;
	libusb_device **listaDispositivos=0;
	libusb_init(&ctx);
	int contador = libusb_get_device_list(ctx, &listaDispositivos);
	int i,id;
	for(i=0; i<contador ; i++){
		libusb_device *dispositivo = listaDispositivos[i];
		for(id=0 ; id<4 ; id++){
			if(comprobacionDeDispositivo(dispositivo, idProvedor, arregloIdProductos[id]) == 0){
				libusb_device_handle *dispositivoManejable;
				libusb_open(dispositivo, &dispositivoManejable);
				servo = PARAMETER_SERVO0_MIN+(9*servo);
				rangoMin = rangoMin/16;
				libusb_control_transfer(dispositivoManejable,0x40,REQUEST_SET_PARAMETER,rangoMin,servo,0,0,(ushort)5000);
				libusb_close(dispositivoManejable);
				break;
			}
		}
	}
	libusb_free_device_list(listaDispositivos, 0);
	libusb_exit(ctx);
	return 0;
}

/************************************************************/

int asignarRangoMax(float rangoMax, int servo){
	const unsigned short idProvedor = 0x1ffb;
	unsigned short arregloIdProductos[]={0x0089, 0x008a, 0x008b, 0x008c};	
	libusb_context *ctx=0;
	libusb_device **listaDispositivos=0;
	libusb_init(&ctx);
	int contador = libusb_get_device_list(ctx, &listaDispositivos);
	int i,id;
	for(i=0; i<contador ; i++){
		libusb_device *dispositivo = listaDispositivos[i];
		for(id=0 ; id<4 ; id++){
			if(comprobacionDeDispositivo(dispositivo, idProvedor, arregloIdProductos[id]) == 0){
				libusb_device_handle *dispositivoManejable;
				libusb_open(dispositivo, &dispositivoManejable);
				servo = PARAMETER_SERVO0_MAX+(9*servo);
				rangoMax = rangoMax/16;
				libusb_control_transfer(dispositivoManejable,0x40,REQUEST_SET_PARAMETER,rangoMax,servo,0,0,(ushort)5000);
				libusb_close(dispositivoManejable);
				break;
			}
		}
	}
	libusb_free_device_list(listaDispositivos, 0);
	libusb_exit(ctx);
	return 0;
}

